#!/usr/bin/python

import roslib; roslib.load_manifest('efri')
import rospy

import zenither.zenither as zen

if __name__ == '__main__':
    zed = zen.Zenither(robot='HRL2')

    while(not rospy.is_shutdown()):
        curr_height = zed.get_position_meters()
        height = raw_input('Current height is: '+ str(curr_height) + '. Enter desired height: ')
        zed.torque_move_position(float(height))
